﻿#include "RobotHandTest.h"
#include "common_log.h"
#include <QTimer>
constexpr int g_dataLength = 64;
constexpr int g_stateLength = 80;

RobotHandTest::RobotHandTest(QObject* parent)
	: RobotHand(parent)
{
	m_brand = "TEST";
	log_info("-----------------TEST Robot has been create-------------");
	m_stateTimer = new QTimer();
	m_stateTimer->setInterval(50);
	connect(m_stateTimer, &QTimer::timeout, this, &RobotHandTest::SendRobotStateData);

	m_pos.x = 10;
	m_pos.y = 20;
	m_pos.z = 30;
	m_pos.a = 0;
	m_pos.b = 0;
	m_pos.c = 180;
}

RobotHandTest::~RobotHandTest()
{}

// 初始
int RobotHandTest::Init(const RobotParam& param)
{
	// 参数校验
	if (param.id.isEmpty() || param.ip.isEmpty()) {
		log_error("Kuka robot init param error, id is [{}] or ip is [{}]", param.id, param.ip);
		return -1;
	}
	if (param.statusPort <= 0 || param.cmdPort <= 0) {
		log_error("Kuka robot init prot error, state is [{}] or cmd is [{}]", param.statusPort, param.cmdPort);
		return -1;
	}
	m_pos = param.base;
	emit ConnectionStateChanged(0);	// 连接成功

	m_stateTimer->start();

	return 0;
}

/****************************************************************************
* 功能: 重连接口
* 传参:
* 返回值: 重连是否成功返回0， 状态端口失败返回-1，命令端口失败返回-2, 全部失败返回-3
****************************************************************************/
int RobotHandTest::Reconnecte()
{

	return 0;
}
int RobotHandTest::GetConnectedState()
{
	return 0;
}
/****************************************************************************
* 功能: 停止接口，机械手停止运动并清理运动轨迹
* 传参:
* 返回值:
****************************************************************************/
bool RobotHandTest::Stop()
{
	m_stop = true;
	return true;
}

/****************************************************************************
	* 功能: 暂停/恢复接口，机械手暂停，不清理轨迹，机械手恢复后继续运动
	* 传参:
	* 返回值:
	****************************************************************************/
bool RobotHandTest::Pause()
{
	m_pause = true;
	return true;
}
bool RobotHandTest::Resume()
{
	m_stop = false;
	m_pause = false;
	return true;
}

/****************************************************************************
 * 功能: 复位机械手，清除机械手程序错误，以及点位信息
 * 传参:
 * 返回值:
 ****************************************************************************/
bool RobotHandTest::Reset()
{

	return true;
}

/****************************************************************************
	* 功能: 设置和获取当前的TCPBase(未启用)
	* 传参:
	* 返回值:
	****************************************************************************/
bool RobotHandTest::SetTcpBase(const QString& info)
{
	return false;
}
QString RobotHandTest::GetTcpBase()
{
	return "";
}

/****************************************************************************
	* 功能: 设置DO
	* 传参: index: DO编号(1-n), value: true=1,false=0;
	* 返回值:
	****************************************************************************/
bool RobotHandTest::SetDo(unsigned int index, bool value)
{
	if (index > 32 || index < 1) {
		log_error("failed, index {} error", index);
		return false;
	}
	m_do[index - 1] = value ? '1' : '0';
	return true;
}

/****************************************************************************
	* 功能: 夹具动作/预定义动作
	* 传参: index: 预先定义的动作编号（由机械手内部程序定义）
	* 返回值:
	****************************************************************************/
bool RobotHandTest::SetGrip(unsigned int index, const GripParam& param)
{
	log_error("set grip index = {}", index);

	return true;
}

/****************************************************************************
	* 功能: 移动指令
	* 传参:
	* 返回值:
	****************************************************************************/
bool RobotHandTest::MoveJL(const MoveJLInfo& move)
{
	if (!m_path.isEmpty()) {
		log_error("Thera are unfinished path");
		return false;
	}

	m_path.append(QPair(move.id, move.pos));


	return true;
}

/****************************************************************************
	* 功能: 特殊指令
	* 传参:
	* 返回值:
	****************************************************************************/
bool RobotHandTest::ExecCmd(const QString& cmd, const QString& para, QString& result)
{
	return false;
}

void RobotHandTest::SendRobotStateData()
{
	// 发送关节角
	emit RobotHandJoints("123");
	emit RobotHandToolID(1);
	emit RobotHandFaultCode(0, "");
	emit RobotHandDIInfo(m_di);
	emit RobotHandDOInfo(m_do);

	if (m_path.isEmpty()) {
		emit RobotHandMoving(0);
		emit RobotHandPointId(0);
	} else {
		times++;
		emit RobotHandMoving(1);
		if (times % 20 == 0) {
			times = 0;
			auto point = m_path.takeFirst();
			m_pos = point.second;
			emit RobotHandPointId(point.first);
		} else {
			emit RobotHandPointId(0);
		}
	}
	PositionJ posj = m_pos.TransToPositonJ();

	emit RobotHandTargets(QString(posj.toByteArray()));
}
